import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Path
import math

class FollowPathNode(Node):

    def __init__(self):
        super().__init__('follow_path_node')

        # 订阅local_path话题，消息类型为Path
        self.path_subscription = self.create_subscription(
            Path,
            'local_path',
            self.path_callback,
            10)
        self.path_subscription  # 防止未使用的变量警告

        # 创建一个发布器来发布速度指令（cmd_vel），消息类型为Twist
        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 10)

        # 初始化路径点列表和当前目标点索引
        self.path_points = []
        self.current_target_index = 0
        self.is_moving = False
        self.has_received_path = False

        # 设置最大线速度和角速度
        self.max_linear_velocity = 1.0  # 最大线速度（米/秒）
        self.max_angular_velocity = 1.0  # 最大角速度（弧度/秒）

        # 创建一个定时器，每0.1秒调用一次控制器方法
        self.timer = self.create_timer(0.1, self.control_loop)

        self.get_logger().info('FollowPathNode has been initialized.')

    def path_callback(self, msg):
        """路径订阅的回调函数。更新当前路径点列表。"""
        self.get_logger().info(f'Received path with {len(msg.poses)} points.')
        self.path_points = msg.poses
        self.current_target_index = 0  # 重置目标点索引
        self.is_moving = True  # 开始移动
        self.has_received_path = True  # 标记为接收到路径数据

    def control_loop(self):
        """控制循环：处理路径点并发布速度指令。"""
        if not self.has_received_path:
            # 如果未收到路径数据，保持停止状态
            self.publish_stop()
            return

        if not self.path_points or not self.is_moving:
            # 如果路径点为空或未开始移动，不执行任何操作
            self.publish_stop()
            return

        # 获取当前目标点
        if self.current_target_index >= len(self.path_points):
            self.get_logger().info('Reached the end of the path.')
            self.stop_moving()
            self.publish_stop()  # 确保路径结束后停止
            self.has_received_path = False  # 重置路径接收标记
            self.timer.cancel()  # 停止定时器
            return

        target_pose = self.path_points[self.current_target_index].pose

        # 调试信息：目标点的位置
        self.get_logger().info(f'Current goal position: x={target_pose.position.x}, y={target_pose.position.y}')

        # 计算机器人当前到目标点的距离和角度差
        linear_vel, angular_vel = self.compute_velocity(target_pose)

        # 对线速度和角速度进行最大值约束
        linear_vel = min(max(linear_vel, -self.max_linear_velocity), self.max_linear_velocity)
        angular_vel = min(max(angular_vel, -self.max_angular_velocity), self.max_angular_velocity)

        # 调试信息：计算后的速度
        self.get_logger().info(f'Constrained velocities -> Linear: {linear_vel:.2f} m/s, Angular: {angular_vel:.2f} rad/s')

        # 创建Twist消息并发布速度
        twist = Twist()
        twist.linear.x = linear_vel
        twist.angular.z = angular_vel
        self.cmd_vel_publisher.publish(twist)

        # 如果目标点到达或距离非常近，移动到下一个目标点
        if self.is_at_target(target_pose):
            self.get_logger().info(f'Reaching target point {self.current_target_index}. Stopping and moving to the next point.')
            self.stop_moving()  # 停止移动
            self.current_target_index += 1  # 移动到下一个目标点
            self.is_moving = True  # 设置为移动状态以便发布下一目标点的速度

    def compute_velocity(self, target_pose):
        """计算线速度和角速度。"""
        # 获取机器人当前位置，这里假设在(0,0)方向为0（在实际情况下，需要从位置获取或者odom话题获取）
        robot_x = 0.0
        robot_y = 0.0
        robot_theta = 0.0

        # 调试信息：机器人当前位置
        self.get_logger().info(f'Robot position -> x: {robot_x}, y: {robot_y}, theta: {robot_theta}')

        # 计算目标点相对于机器人的局部坐标
        dx = target_pose.position.x - robot_x
        dy = target_pose.position.y - robot_y

        # 计算目标距离和角度
        distance = math.sqrt(dx ** 2 + dy ** 2)
        angle_to_goal = math.atan2(dy, dx)

        # 调试信息：目标的相对位置和距离
        self.get_logger().info(f'Relative goal position -> dx: {dx}, dy: {dy}, distance: {distance:.2f}')

        # 计算角度差
        angle_diff = angle_to_goal - robot_theta

        # 设置简单的比例控制器
        k_linear = 0.5  # 线速度比例增益
        k_angular = 2.0  # 角速度比例增益

        # 计算线速度和角速度
        linear_vel = k_linear * distance
        angular_vel = k_angular * angle_diff

        # 调试信息：计算的角度差
        self.get_logger().info(f'Angle to goal: {angle_to_goal:.2f}, Angle difference: {angle_diff:.2f}')

        return linear_vel, angular_vel

    def is_at_target(self, target_pose):
        """检查是否到达目标点。"""
        robot_x = 0.0
        robot_y = 0.0

        dx = target_pose.position.x - robot_x
        dy = target_pose.position.y - robot_y
        distance = math.sqrt(dx ** 2 + dy ** 2)

        # 设置到达目标点的距离阈值
        threshold = 0.1  # 0.1米

        return distance < threshold

    def stop_moving(self):
        """停止移动，发布零速度指令。"""
        self.publish_stop()

    def publish_stop(self):
        """发布停止指令。"""
        twist = Twist()
        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.cmd_vel_publisher.publish(twist)
        self.get_logger().info('Stopped moving.')

def main(args=None):
    rclpy.init(args=args)
    follow_path_node = FollowPathNode()

    try:
        rclpy.spin(follow_path_node)
    except KeyboardInterrupt:
        follow_path_node.get_logger().info('Keyboard Interrupt (SIGINT)')
    except Exception as e:
        follow_path_node.get_logger().error(f'Exception: {e}')
    finally:
        follow_path_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
